/**
 * @file data_utils.h
 * @brief Provide a few widely used function for basic type
 */
#ifndef JPS_DATA_UTIL_H
#define JPS_DATA_UTIL_H

#include <util/data_type.h>

///Heap element comparison
template<class T>
struct compare_state {
  bool operator()(T a1, T a2) const {
    double f1 = a1->g + a1->h;
    double f2 = a2->g + a2->h;
    if ((f1 >= f2 - 0.000001) && (f1 <= f2 + 0.000001))
      return a1->g < a2->g; // if equal compare gvals
    return f1 > f2;
  }
};

///Template for transforming a vector
template <class T, class TF>
VectorEigen<T> transform_vec(const VectorEigen<T> &t, const TF &tf) {
  VectorEigen<T> new_t;
  for (const auto &it : t)
    new_t.push_back(tf * it);
  return new_t;
}

///Template for calculating distance
template <class T>
float_decimal_t total_distance(const VectorEigen<T>& vs){
  float_decimal_t dist = 0;
  for(unsigned int i = 1; i < vs.size(); i++)
    dist += (vs[i] - vs[i-1]).norm();

  return dist;
}

///Transform all entries in a vector using given TF
#define transform_vec3 transform_vec<Vec3f, Aff3f>
///Sum up total distance for VectorPoint2f
#define total_distance2f total_distance<Vec2f>
///Sum up total distance for VectorPoint3f
#define total_distance3f total_distance<Vec3f>
///Sum up total distance for VectorPoint2i
#define total_distance2i total_distance<Point2i>
///Sum up total distance for vec_Vec3i
#define total_distance3i total_distance<Vec3i>
#endif
